#include <netlink/netlink.h>
#include <netlink/genl/genl.h>
#include <netlink/genl/ctrl.h>
#include <netlink/socket.h>
#include <stdio.h>
#include <stdlib.h>
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libswscale/swscale.h>
#include <SDL.h>
#include <SDL_thread.h>
#include <pthread.h>
#include "time.h"
#include "common.h"
#include <linux/videodev.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

/*****************************************************************************/
/********************************* MACROS ************************************/
/*****************************************************************************/
#define DECODE_QUEUE_SIZE	10
#define Z_Z_Z_Z_Z_Z_Z	100
#define WAIT_TIME_BETWEEN_FRAMES_IN_MS	20


/*****************************************************************************/
/********************************* Typedefs **********************************/
/*****************************************************************************/

typedef struct
{
	uint8_t 	*buffers[DECODE_QUEUE_SIZE];
	AVFrame 	*frames[DECODE_QUEUE_SIZE];
	int 		first_frame;
	int 		last_frame;
	int 		count;
	SDL_mutex 	*mutex;
} decoded_queue;

/*****************************************************************************/
/******************************** PROTOTYPES *********************************/
/*****************************************************************************/

int 	init_camera();
void 	*get_frame(void* arg);
void 	*decode_thread(void* arg);
void 	img_convert(AVPicture * target , int targetFmt, AVPicture * source ,int sourceFmt,int w, int h);
int 	close_camera();

void 	enqueue_frame();
AVFrame *read_last_frame();
void 	dequeue_frame();
AVFrame *read_first_frame();
void daemonize();


/*****************************************************************************/
/***************************** NETLINK DEFINITIONS ***************************/
/*****************************************************************************/

int send_msg_fmt(void *buff, int len);
int send_msg_streamon();
int send_msg_frame(void *buff, int len);
int send_msg_close();
int send_msg(void *buff, int len, int attr);
int seng_msg_frame_finish();

/* Attributes */
static struct nla_policy ex_policy[MAX_ATTR_INDEX+1] =
{	  [STREAMON_ATTR] 	= { .type = NLA_UNSPEC },
	  [STREAMOFF_ATTR]	= { .type = NLA_UNSPEC },
	  [FRAME_ATTR]		= { .type = NLA_UNSPEC },
	  [RETURN_ATTR]		= { .type = NLA_UNSPEC }
};

/*****************************************************************************/
/*************************** GLOBAL VARIABLES ********************************/
/*****************************************************************************/
int stream_on = 0;
AVFormatContext *pFormatCtx;
int             videoStream;
AVCodecContext  *pCodecCtx;
int             numBytes;
decoded_queue	dcd_queue;

pthread_cond_t  cond = PTHREAD_COND_INITIALIZER;
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t nl_mutex = PTHREAD_MUTEX_INITIALIZER;

time_t					secs;
struct v4l2_format fmt;

int connected = 1;            // Connection flag.

static int 				global_nl_family;             // Netlink family.
static struct nl_handle *global_nl_sock; // Netlink socket.
static int 				global_nl_rv = 0;


/*****************************************************************************/
/***************************** NETLINK CALLBACK ******************************/
/*****************************************************************************/
static int parse_cb(struct nl_msg *msg, void *arg)
{

	struct nlmsghdr *nlh = nlmsg_hdr(msg);
    struct nlattr *attrs[MAX_ATTR_INDEX + 1];

    /* Validate message and parsevoid init_camera() attributes */
    genlmsg_parse(nlh, 0, attrs, MAX_ATTR_INDEX + 1, ex_policy);

	/* STREAMON Request */
    if(attrs[STREAMON_ATTR])
    {
    	stream_on = 1;
    	send_msg_streamon();
    }
    else if(attrs[G_FMT_ATTR])
    {
    	send_msg_fmt(&fmt, sizeof(struct v4l2_format));
    }
	/* STREAMOFF Request */
	else if (attrs[STREAMOFF_ATTR])
	{
		int ret = 1;
    	stream_on = 0;
//		send_ret_msg(&ret);
	 }

	/* FRAME Request */
	else if(attrs[FRAME_ATTR])
	{
		if(stream_on)
		{
			pthread_mutex_lock(&mutex);
			pthread_cond_signal(&cond);
			pthread_mutex_unlock(&mutex);
		}
	}

     return NL_OK;
 }

/*****************************************************************************/
/***************************** NETLINK FUNCTIONS *****************************/
/*****************************************************************************/
int send_msg_close()
{
	int tr = 1;
	return send_msg(&tr, 1, KILL_ATTR);
}
/* Sends a "message" type message */
int send_msg_streamon()
{
	int tr = 1;
	return send_msg(&tr, 1,STREAMON_ATTR);
}

/* Sends a "message" type message */
int send_msg_fmt(void *buff, int len)
{
	return send_msg(buff,len,G_FMT_ATTR);
}


/* Sends a "message" type message */
int send_msg_frame(void *buff, int len)
{
	return send_msg(buff,len,FRAME_ATTR);
}

int seng_msg_frame_finish()
{
	int tr = 1;
	return send_msg(&tr, sizeof(int), RETURN_ATTR);
}

/* General sending message function */
int send_msg(void *buff, int len, int attr)
{
	struct nl_msg *msg;
	int *vrv;
   // PUS_DEBUG("sending message  %d\n", stamp);

	msg = nlmsg_alloc();
	ERROR_ON(msg == NULL, "Error allocating message.\n");

	vrv = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, global_nl_family, 0, NLM_F_ECHO, STD_CMD, 1);
	ERROR_ON(vrv == NULL, "Error - genlmsg returned NULL!\n");

	global_nl_rv = nla_put(msg, attr, len, buff);
	ERROR_ON(global_nl_rv < 0, "Error - nla_put_string returned negative result!\n");

	/* Send message over netlink socket */
	global_nl_rv = nl_send_auto_complete(global_nl_sock, msg);
	ERROR_ON(global_nl_rv < 0, "Error sending the message (%d).\n", global_nl_rv);

	/* Free message */
	nlmsg_free(msg);

	return 1;

err:
	return -1;
}

/*****************************************************************************/
/***************************** QUEUE RELATED FUNC ****************************/
/*****************************************************************************/


void enqueue_frame()
{
	int nextIndex = (dcd_queue.last_frame + 1) % DECODE_QUEUE_SIZE;
	SDL_LockMutex(dcd_queue.mutex);
	dcd_queue.last_frame = nextIndex;
	dcd_queue.count++;
	if(dcd_queue.first_frame == -1)
		dcd_queue.first_frame = 0;
	DEBUG("ENQUEUE: adding: count:%d!\n",dcd_queue.count);
	SDL_UnlockMutex(dcd_queue.mutex);
}

AVFrame *read_last_frame()
{
	while(dcd_queue.first_frame == dcd_queue.last_frame && dcd_queue.count > 0)
	{
		SDL_UnlockMutex(dcd_queue.mutex);
		DEBUG( "read last frame: Sleeping\n");
		SDL_Delay(Z_Z_Z_Z_Z_Z_Z);
		SDL_LockMutex(dcd_queue.mutex);
	}
	return dcd_queue.frames[dcd_queue.last_frame];
}


uint8_t *read_first_buffer()
{
	int retIndex;
	SDL_LockMutex(dcd_queue.mutex);
	while(dcd_queue.count == 0)
	{
		SDL_UnlockMutex(dcd_queue.mutex);
		SDL_Delay(10);
		SDL_LockMutex(dcd_queue.mutex);
	}
	retIndex = dcd_queue.first_frame;
	SDL_UnlockMutex(dcd_queue.mutex);
	return dcd_queue.buffers[retIndex];
}

void dequeue_frame()
{
	SDL_LockMutex(dcd_queue.mutex);
	dcd_queue.first_frame += 1;
	dcd_queue.first_frame %= DECODE_QUEUE_SIZE;
	dcd_queue.count--;
	DEBUG("DEQUEUE: dequeueing: count:%d!\n",dcd_queue.count);
	SDL_UnlockMutex(dcd_queue.mutex);
}


/*****************************************************************************/
/***************************** VIDEO RELATED FUNC ****************************/
/*****************************************************************************/
int init_camera(const char *video_file)
{
	AVFrame 		*m_pFrameYUV;
	uint 			i;
	uint8_t 		*m_buffer;
	AVCodec         *pCodec;

	// Register all formats and codecs
	av_register_all();

	if(SDL_Init(SDL_INIT_VIDEO | SDL_INIT_AUDIO | SDL_INIT_TIMER))
	{
		DEBUG( "Could not initialize SDL - %s\n", SDL_GetError());
		return -1;
	}

	// Open video file
	if(av_open_input_file(&pFormatCtx, video_file, NULL, 0, NULL)!=0)
		return -1; // Couldn't open file

	// Retrieve stream information
	if(av_find_stream_info(pFormatCtx)<0)
		return -1; // Couldn't find stream information

	// Find the first video stream
	videoStream=-1;

	for(i=0; i < pFormatCtx->nb_streams; i++)
	{
		if(pFormatCtx->streams[i]->codec->codec_type==CODEC_TYPE_VIDEO)
		{
			videoStream=i;
			break;
		}
	}

	if(videoStream==-1)
		return -1; // Didn't find a video stream

	// Get a pointer to the codec context for the video stream
	pCodecCtx=pFormatCtx->streams[videoStream]->codec;
	fmt.fmt.pix.width = pCodecCtx->width;
	fmt.fmt.pix.height = pCodecCtx->height;
	fmt.fmt.pix.pixelformat = PIX_FMT_YUV420P;

	// Find the decoder for the video stream
	pCodec=avcodec_find_decoder(pCodecCtx->codec_id);
	if(pCodec==NULL)
	{
		ERROR( "Unsupported codec!\n");
		return -1; // Codec not found
	}

	// Open codec
	if(avcodec_open(pCodecCtx, pCodec)<0)
		return -1; // Could not open codec

	// init the queue
	for(i=0; i<DECODE_QUEUE_SIZE; i++)
	{
		// Allocate an AVFrame structure
		m_pFrameYUV=avcodec_alloc_frame();
		if(m_pFrameYUV==NULL)
		{
			ERROR("Couldn't allocate frame.\n");
			return -1;
		}

		// Determine required buffer size and allocate buffer
		numBytes=avpicture_get_size(PIX_FMT_YUV420P, pCodecCtx->width, pCodecCtx->height);
		m_buffer=(uint8_t *)av_malloc(numBytes*sizeof(uint8_t));
		if(!m_buffer)
		{
			ERROR("Couldn't allocate frame.\n");
			return -1;
		}

		avpicture_fill((AVPicture *)m_pFrameYUV, m_buffer, PIX_FMT_YUV420P, pCodecCtx->width, pCodecCtx->height);
		dcd_queue.frames[i] = m_pFrameYUV;
		dcd_queue.buffers[i] = m_buffer;
	}
	dcd_queue.first_frame = -1;
	dcd_queue.last_frame = 0;
	dcd_queue.count = 0;
	dcd_queue.mutex = SDL_CreateMutex();

	return 0;
}

void *decode_thread(void *args)
{
	AVPacket        m_packet;
	int 			frame_index;
	AVFrame         *m_pFrame, *pFrame;
	int             frameFinished, m_numBytes;
	uint8_t			*m_buffer;

	pFrame=avcodec_alloc_frame();
	if(pFrame== NULL)
	{
		ERROR("Couldn't allocate frame.\n");
		return NULL;
	}
	m_numBytes=avpicture_get_size(pCodecCtx->pix_fmt, pCodecCtx->width, pCodecCtx->height);
	m_buffer=(uint8_t *)av_malloc(m_numBytes*sizeof(uint8_t));
	if(m_buffer== NULL)
	{
		ERROR("Couldn't allocate frame.\n");
		return NULL;
	}
	avpicture_fill((AVPicture *)pFrame, m_buffer, pCodecCtx->pix_fmt, pCodecCtx->width, pCodecCtx->height);

	frame_index = 0;
	m_pFrame = NULL;
	// Read frames and save first five frames to disk
	secs = time(NULL);
	while(1)
	{
		while(av_read_frame(pFormatCtx, &m_packet)>=0)
		{
			if( m_pFrame == NULL)
				m_pFrame = read_last_frame();
			// Is this a packet from the video stream?
			if(m_packet.stream_index==videoStream)
			{
				// Decode video frame
				avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, &m_packet);

				// Did we get a video frame?
				if(frameFinished)
				{
					img_convert((AVPicture *)m_pFrame, PIX_FMT_YUV420P, (AVPicture *)pFrame, pCodecCtx->pix_fmt, pCodecCtx->width, pCodecCtx->height);
					enqueue_frame();
					m_pFrame = NULL;
				}
				frame_index++;
				if( frame_index % 50 == 0)
					DEBUG("Camera: frames per second: %f\n", (float)frame_index /(time(NULL) - secs) );
			}
		}
		// rewind
		av_seek_frame(pFormatCtx, videoStream, 0,AVSEEK_FLAG_BACKWARD);
	}
	return NULL;
}



void *get_frame(void *arg)
{
	frame_packet frm_packet;
	int offset = 0;
	unsigned int bytes_copied = 0;
	uint8_t 	*m_buffer;

	// Read frames and save first five frames to disk
	secs = time(NULL);
	while(1)
	{
		// wait for frame request
		pthread_cond_wait(&cond, &mutex);
		m_buffer = read_first_buffer();
		offset = 0;
		while(offset < numBytes)
		{
			bytes_copied = MIN(FRAME_BUFFER_SIZE, numBytes - offset);
			memcpy(frm_packet.buffer, m_buffer + offset, bytes_copied);
			offset += bytes_copied;
			frm_packet.length = bytes_copied;
			frm_packet.frame_finito = (offset >= numBytes);
			send_msg_frame(&frm_packet, sizeof(frame_packet));
		}
		seng_msg_frame_finish();
		SDL_Delay(WAIT_TIME_BETWEEN_FRAMES_IN_MS);
		dequeue_frame();
	}
	pthread_mutex_unlock(&mutex);

	close_camera();

	return NULL;
}

int close_camera()
{
	send_msg_close();
	// Close the codec
	avcodec_close(pCodecCtx);

	// Close the video file
	av_close_input_file(pFormatCtx);

	return 0;
}

void img_convert(AVPicture * target , int targetFmt, AVPicture * source ,int sourceFmt,int w, int h)
{
	static struct SwsContext *img_convert_ctx=NULL;

	if(img_convert_ctx == NULL)
	{
		img_convert_ctx = sws_getContext(w, h, sourceFmt, w, h, targetFmt, SWS_BICUBIC,NULL, NULL, NULL);
	}

	sws_scale(img_convert_ctx, (const uint8_t * const*)source->data, source->linesize, 0, h, target->data, target->linesize);
}

/*****************************************************************************/
/************************************ MAIN ***********************************/
/*****************************************************************************/
void daemonize()
{
	int i;
	if(getppid() == 1)
		return;
	i = fork();
	if(i<0)
		exit(1);
	if( i>0)
		exit(0);
	setsid();
	for(i=getdtablesize(); i>=0; --i)
	{
		close(i);
	}
	i = open("/dev/null", O_RDWR);
	dup(i);
	dup(i);
}

int main(int argc, char *argv[])
{
	pthread_t 		frames_thread;
	pthread_t		decode_thread_ptr;
	int thread_err;

	if( argc < 2)
	{
		ERROR("Must supply video file\n");
		return -1;
	}

	/* making deamon */
	daemonize();

	pthread_mutex_init(&mutex, NULL);
	pthread_cond_init (&cond, NULL);
	pthread_mutex_lock(&mutex);


	if(init_camera(argv[1]))
		return -1;

	if( (thread_err=pthread_create( &decode_thread_ptr, NULL, decode_thread, NULL) ))
	{
		ERROR("Thread frames_operation creation failed!\n");
	    return NL_STOP;
	}

	if( (thread_err=pthread_create( &frames_thread, NULL, get_frame, NULL) ))
	{
		ERROR("Thread frames_operation creation failed!\n");
	    return NL_STOP;
	}

	/* Allocate a new netlink socket */
	global_nl_sock = nl_handle_alloc();
	ERROR_ON(global_nl_sock == NULL, "Failed allocating socket.\n");

	/* Netlink configuration */
	nl_disable_sequence_check(global_nl_sock);
	nl_socket_set_local_port(global_nl_sock, NETLINK_PID_SOCK);
	global_nl_rv = genl_connect(global_nl_sock);
	ERROR_ON(global_nl_rv, "Error connecting the socket (%d).\n", global_nl_rv);
	global_nl_family = genl_ctrl_resolve(global_nl_sock, FAMILY_NAME);
	ERROR_ON(global_nl_family < 0, "Error resolving family (%d).\n", global_nl_family);

	nl_socket_modify_cb(global_nl_sock, NL_CB_VALID, NL_CB_CUSTOM, parse_cb, NULL);

	/* Connection loop */
	send_msg(&connected, sizeof(int),INIT_ATTR);

	while(connected)
	{
		nl_recvmsgs_default(global_nl_sock);
		nl_wait_for_ack(global_nl_sock);
	}

	return 1;

	err:
		return -1;

}
